#ifndef BC_CONTACT_TASK_H
#define BC_CONTACT_TASK_H

#include "srpq_controller/AdvanceControllers/BalanceController/BalanceCtrlTask.hpp"
#include <common/Controllers/FootSwingTrajectory.h>

template <typename T>
struct BalanceContactData
{
    Vec3<T> OQ_init;
    Vec3<T> OP_init;
    Vec3<T> QP_O;
    Vec3<T> QP_Q;
    Vec3<T> OQ_now;
};

template <typename T>
class ContactTask : public BalanceCtrlTask<T> {
public:
    ContactTask();
    ~ContactTask(){}
private:
    T _leg_height = 0.3;
    bool firstContact[4];
    Mat3<T> Rot;
    BalanceContactData<T> _contact_data[4];
    FootSwingTrajectory<T> footContactTrajectories[4];
    void  task(ControlFSMData<T>& data, BalanceCtrlData<T>* ctrl_data, BCoutputData<T>* BCout, int leg);
    void calcurateLegData(T roll, T pitch, T yaw);
};

#endif
